#ifndef __ASTAR__LOCALPATH_LIB__
#define __ASTAR__LOCALPATH_LIB__
#include <iostream>
#include <map>
#include <vector>
#include <queue>
#include <algorithm>
#include "priorityqueueimpl.h"
#include <memory>

using namespace std;

class Point{
private:
	int x;
	int y;
public:
	Point(){
		x=0;
		y=0;
	}

	Point(int x, int y){
		this->x=x;
		this->y=y;
	}

	int getX() const{
		return x;
	}

	int getY() const{
		return y;
	}

	void setX(int x){
		this->x=x;
	}

	void setY(int y){
		this->y=y;
	}

    unsigned int hashCode() const{
		int hash = 5;
        hash = 59 * hash + this->x;
        hash = 59 * hash + this->y;
        return hash;
    }

	bool operator <(const Point& rhs) const
    {
		return getX()<rhs.getX()|| (!(rhs.getX()<getX()) && getY()<rhs.getY()); 
    }
};

class AStarPath{
public:
    shared_ptr<Point>point;
    float f;
    float g;
    shared_ptr<AStarPath>parent;

    AStarPath() {
        g= 0.0;
		f= 0.0;
    }

    AStarPath(shared_ptr<AStarPath>p) {
        parent = p;
        g = p->g;
        f = p->f;
    }

    /**
     * Compare to another object using the total cost f.
     *
     * @param o The object to compare to.
     * @see Comparable#compareTo()
     * @return <code>less than 0</code> This object is smaller *
     * than <code>0</code>; <code>0</code> Object are the same.
     * <code>bigger than 0</code> This object is bigger than o.
     */
    int compareTo(shared_ptr<AStarPath>o) {
        return (int) (f - o->f);
    }

    /*
     * Get the last point on the path.
     * @return The last point visited by the path.
     */
    shared_ptr<Point>getPoint() {
        return point;
    }

    /**
     * Set the point
     */
    void setPoint(shared_ptr<Point>p) {
        point = p;
    }
};


class AstarQueueComparator:public QueueComparator{
public:
	virtual int compareTo(void*lhs, void*rhs){
		AStarPath* x=(AStarPath*)lhs;
		AStarPath* y=(AStarPath*)rhs;
		return (int)(x->f - y->f);
	}
};


class AStarAbstract{
private:
	bool fullPathCalculated;
	std::map<Point,float> mindists;
	float lastCost;
	int expandedCounter;
	float closestLength;
	shared_ptr<Point>lastClosestPoint;
	std::vector<shared_ptr<Point>>successorsList;
	PriorityQueue<shared_ptr<AStarPath>>paths;
	AstarQueueComparator comparator;
	//int indexi;
	//int pccount;
	//FILE*file;
public:
	AStarAbstract(){
		closestLength=999999999999999999999.0f;
		expandedCounter=0;
		lastCost=0.0;

		paths.setComparator(&comparator);
		//indexi=0;
		//pccount=0;
		//file=fopen("d:\\cpp.txt","w");
	}

	int getExpandedCounter(){
		return expandedCounter;
	}

	/*int getPcCount(){
		return pccount;
	}*/

	std::vector<shared_ptr<Point>> compute(shared_ptr<Point>start) {
        if (isGoal(start)) {
            std::vector<shared_ptr<Point>> list;
			list.push_back(start);
            return list;
        }
		shared_ptr<AStarPath>root = shared_ptr<AStarPath>(new AStarPath());
		//pccount++;

        root->setPoint(start);
        /* Needed if the initial point has a cost.  */
        f(root, start, start);
        expand(root);
        for (;;) {
			shared_ptr<AStarPath>p = paths.poll();
		//	fprintf(file,"poll-");
			//printAstar(p);
			//fprintf(file,"After poll\n");
			//printqueue();
			//fprintf(file,"\n\n\n\n");
			if(p==NULL){
				lastCost = 999999999999999999999999999999.0f;
                return std::vector<shared_ptr<Point>>();//
			}

            shared_ptr<Point>last = p->getPoint();
            lastCost = p->g;
            if (isGoal(last)) {
				std::vector<shared_ptr<Point>>retPath;
                for (shared_ptr<AStarPath>i = p; i != NULL; i = i->parent) {
					retPath.push_back(i->getPoint());
                }
				std::reverse(retPath.begin(), retPath.end());
                return retPath;
            }

            expand(p);
        }
    }


protected:
	bool virtual isGoal(shared_ptr<Point>node)=0;
	virtual void generateSuccessors(shared_ptr<Point>node, std::vector<shared_ptr<Point>>*successorsOutputList)=0;
	virtual float g(shared_ptr<Point>from, shared_ptr<Point>to)=0;
	virtual float h(shared_ptr<Point>from, shared_ptr<Point>to)=0;
	virtual float squaredLengthToGoal(shared_ptr<Point>cell)=0;
	
	virtual float f(shared_ptr<AStarPath>p, shared_ptr<Point>from, shared_ptr<Point>to) {
        float tg = (float)(g(from, to) + ((p->parent != NULL) ? p->parent->g : 0.0f));
        float th = h(from, to);
        p->g = tg;
        p->f = tg + th;
        return p->f;
    }

	/**
     * Expand a path.
     *
     * @param path The path to expand.
     */
    void expand(shared_ptr<AStarPath>path) {
        shared_ptr<Point>p = path->getPoint();
        float lengthToGoal = squaredLengthToGoal(p);
        if (lengthToGoal < closestLength) {
            closestLength = lengthToGoal;
            lastClosestPoint = p;
        }

		shared_ptr<Point>tp=path->getPoint();
		bool valueNotFound=false;
		std::map<Point,float>::iterator it;
		it=mindists.find(*tp);
		double min=0;
		if(it == mindists.end()){
			valueNotFound=true;
			//fprintf(file,"mindist hash=%u x=%d,y=%d result=null\n",path->getPoint()->hashCode(),path->getPoint()->getX(),path->getPoint()->getY());
		}else{
			min = it->second;
		//	fprintf(file,"mindist hash=%u x=%d,y=%d result=%.2f\n",path->getPoint()->hashCode(),path->getPoint()->getX(),path->getPoint()->getY(), (float)min);
		}
        /*
         * If a better path passing for this point already exists then
         * don't expand it.
         */
		if (valueNotFound || min > path->f) {
			mindists[*(path->getPoint())]= path->f;
		//	fprintf(file,"put mindist hash=%u x=%d,y=%d f=%.2f\n", path->getPoint()->hashCode(), path->getPoint()->getX(), path->getPoint()->getY(), path->f);
        } else {
            return;
        }
        successorsList.clear();
        generateSuccessors(p, &successorsList);
		if(successorsList.size()==0){
			
		}else{
		for (unsigned int i=0;i<successorsList.size();i++) {
			shared_ptr<Point>t=successorsList[i];
			shared_ptr<AStarPath>newPath = shared_ptr<AStarPath>(new AStarPath(path));
		//	pccount++;
            newPath->setPoint(t);
            f(newPath, path->getPoint(), t);
         //   fprintf(file,"offer:");
		//	printAstar(newPath);
			paths.offer(newPath);
		//	fprintf(file,"after offer\n");
		//	printqueue();
        }
		}

        expandedCounter++;
    }

	/*void printAstar(shared_ptr<AStarPath>p){
		if(p!=NULL){
		fprintf(file,"%d: %d %d f=%.2f g=%.2f\n",indexi,p->getPoint()->getX(), p->getPoint()->getY(), p->f,p->g);
		}
	}*/

	//DEBUG
	/*void printqueue(){
		shared_ptr<AStarPath>*pat=paths.getValues();
		//for(int i=0;i<paths.getSize();i++){
		//	shared_ptr<AStarPath>p=pat[i];
		//	printAstar(p);
		//}
		//indexi++;
	}*/

	/**
     * Get the cost to reach the last node in the path.
     *
     * @return The cost for the found path.
     */
    float getCost() {
        return lastCost;
    }

	bool isFullPathCalculated() {
        return fullPathCalculated;
    }
};

#endif